package com.tgh.utils;

import android.util.FloatMath;

public class Matrix44f {

    /**
     * 新建一个4*4的列优先单位矩阵
     */
    public Matrix44f() {
        mMat = new float[16];
        loadIdentity();
    }

    /**
     * 使用给定的值构造一个4*4的列优先矩阵
     * @param dataArray 长度必须为16
     */
    public Matrix44f(float[] dataArray) {
        mMat = new float[16];
        System.arraycopy(dataArray, 0, mMat, 0, mMat.length);
    }

    /**
     * 返回当前矩阵的数据，修改数组数据也会相应修改矩阵
     * @return
     */
    public float[] getArray() {
        return mMat;
    }

    /**
     * 返回Mij的值
     * @param i 行号，以0开始
     * @param j 列号，以0开始
     * @return value 
     */
    public float get(int i, int j) {
        return mMat[i * 4 + j];
    }

    /**
     *设置矩阵的值
     * @param i 行号，以0开始
     * @param j 列号，以0开始
     */
    public void set(int i, int j, float v) {
        mMat[i * 4 + j] = v;
    }

    /**
     * 设置矩阵为单位矩阵
     */
    public void loadIdentity() {
    	for (int i=0;i<mMat.length;i++){
    	    if (i%5==0) {
                mMat[i]=1;
            }else {
                mMat[i]=0;
            }
		}
    }
    
    /**
     * 以给定矩阵的值构造当前矩阵
     * @param src
     */
    public void load(Matrix44f src) {
        System.arraycopy(src.getArray(), 0, mMat, 0, mMat.length);
    }
    /**
     * 以给定矩阵的值构造当前矩阵
     * @param src
     */
    public void load(float[] src) {
        System.arraycopy(src, 0, mMat, 0, mMat.length);
    }

    /**
     * 构造以(x,y,z)向量为轴的旋转矩阵
     * @param 旋转角度
     * @param x 旋转轴的x坐标
     * @param y 旋转轴的y坐标
     * @param z 旋转轴的z坐标
     */
    public void loadRotate(float rot, float x, float y, float z) {
        float c, s;
        mMat[3] = 0;
        mMat[7] = 0;
        mMat[11] = 0;
        mMat[12] = 0;
        mMat[13] = 0;
        mMat[14] = 0;
        mMat[15] = 1;
        rot *= (float) (Math.PI / 180.0f);
        c = FloatMath.cos(rot);
        s = FloatMath.sin(rot);

        float len = FloatMath.sqrt(x * x + y * y + z * z);
        if (!(len != 1)) {
            float recipLen = 1.f / len;
            x *= recipLen;
            y *= recipLen;
            z *= recipLen;
        }
        float nc = 1.0f - c;
        float xy = x * y;
        float yz = y * z;
        float zx = z * x;
        float xs = x * s;
        float ys = y * s;
        float zs = z * s;
        mMat[0] = x * x * nc + c;
        mMat[4] = xy * nc - zs;
        mMat[8] = zx * nc + ys;
        mMat[1] = xy * nc + zs;
        mMat[5] = y * y * nc + c;
        mMat[9] = yz * nc - xs;
        mMat[2] = zx * nc - ys;
        mMat[6] = yz * nc + xs;
        mMat[10] = z * z * nc + c;
    }
    /**
     * 构造以给定的向量为轴的旋转矩阵
     * @param rot 旋转的角度
     * @param vect 旋转的轴
     */
    public void loadRotate(float rot,Vector3f vect) {
        loadRotate(rot, vect.x,vect.y,vect.z);
    }
    
    /**
     *构造沿着坐标轴缩放的矩阵
     * @param x 轴缩放因子
     * @param y 轴缩放因子
     * @param z 轴缩放因子
     */
    public void loadScale(float x, float y, float z) {
        loadIdentity();
        mMat[0] = x;
        mMat[5] = y;
        mMat[10] = z;
    }
    /**
     *构造沿着坐标轴缩放的矩阵
     * @param vect 包含 x,y,z的缩放因子
     */
    public void loadScale(Vector3f vect) {
        loadIdentity();
        mMat[0] = vect.x;
        mMat[5] = vect.y;
        mMat[10] = vect.z;
    }
    /**
     * 构造沿着vect方向缩放的矩阵
     * @param factor 缩放因子
     * @param vect 缩放方向
     */
    public void loadScale(float factor,Vector3f vect) {
        loadScale(factor, vect.x,vect.y,vect.z);
    }
    /**
     * 构造沿着(x,y,z)方向缩放的矩阵
     * @param factor 缩放因子
     * @param x
     * @param y
     * @param z
     */
    public void loadScale(float factor,float x,float y,float z) {
        loadIdentity();
        float one_f=factor-1;
        mMat[0] = 1 +one_f * x * x;
        mMat[1] = one_f * x * y;
        mMat[2] = one_f * x * z;

        mMat[4] = one_f * x * y;
        mMat[5] = 1 + one_f * y * y;
        mMat[6] =one_f * y * z;

        mMat[8] = one_f * x * z;
        mMat[9] = one_f* z * y;
        mMat[10] = 1 + one_f * z * z;
    }
    /**
     *构造一个平移矩阵，坐标系以(x,y,z)为原点
     * @param x
     * @param y
     * @param z
     */
    public void loadTranslate(float x, float y, float z) {
        loadIdentity();
        mMat[12] = x;
        mMat[13] = y;
        mMat[14] = z;
    }
    
    /**
     * 构造一个平移矩阵，坐标系以(x,y,z)为原点
     * @param vect 
     */
    public void loadTranslate(Vector3f vect) {
        loadIdentity();
        mMat[12] = vect.x;
        mMat[13] = vect.y;
        mMat[14] = vect.z;
    }

    /**
     * 根据矩阵的乘法得到矩阵
     * @param lhs left hand side matrix
     * @param rhs right hand side matrix
     */
    public void loadMultiply(Matrix44f lhs, Matrix44f rhs) {
        for (int i = 0; i < 4; i++) {
            float ri0 = 0;
            float ri1 = 0;
            float ri2 = 0;
            float ri3 = 0;
            for (int j = 0; j < 4; j++) {
                float rhs_ij = rhs.get(i, j);
                ri0 += lhs.get(j, 0) * rhs_ij;
                ri1 += lhs.get(j, 1) * rhs_ij;
                ri2 += lhs.get(j, 2) * rhs_ij;
                ri3 += lhs.get(j, 3) * rhs_ij;
            }
            set(i, 0, ri0);
            set(i, 1, ri1);
            set(i, 2, ri2);
            set(i, 3, ri3);
        }
    }

    /**
     * 构建正交投影矩阵
     * 
     * @param l location of the left vertical clipping plane
     * @param r location of the right vertical clipping plane
     * @param b location of the bottom horizontal clipping plane
     * @param t location of the top horizontal clipping plane
     * @param n location of the near clipping plane
     * @param f location of the far clipping plane
     */
    public void loadOrtho(float l, float r, float b, float t, float n, float f) {
        loadIdentity();
        mMat[0] = 2 / (r - l);
        mMat[5] = 2 / (t - b);
        mMat[10] = -2 / (f - n);
        mMat[12] = -(r + l) / (r - l);
        mMat[13] = -(t + b) / (t - b);
        mMat[14] = -(f + n) / (f - n);
    }

    /**
     * 构建正交投影，左上角为原点，x,y轴向右向下为正，near=-1,far=1
     * 
     * @param w location of the right vertical clipping plane
     * @param h location of the bottom horizontal clipping plane
     */
    public void loadOrthoWindow(int w, int h) {
        loadOrtho(0, w, h, 0, -1, 1);
    }

    /**
     * 构建透视投影矩阵
     * 
     * @param l location of the left vertical clipping plane
     * @param r location of the right vertical clipping plane
     * @param b location of the bottom horizontal clipping plane
     * @param t location of the top horizontal clipping plane
     * @param n location of the near clipping plane, must be positive
     * @param f location of the far clipping plane, must be positive
     */
    public void loadFrustum(float l, float r, float b, float t, float n, float f) {
        loadIdentity();
        mMat[0] = 2 * n / (r - l);
        mMat[5] = 2 * n / (t - b);
        mMat[8] = (r + l) / (r - l);
        mMat[9] = (t + b) / (t - b);
        mMat[10] = -(f + n) / (f - n);
        mMat[11] = -1;
        mMat[14] = -2 * f * n / (f - n);
        mMat[15] = 0;
    }

    /**
     * 构建透视投影矩阵
     * @param fovy vertical field of view angle in degrees
     * @param aspect aspect ratio of the screen
     * @param near near cliping plane, must be positive
     * @param far far clipping plane, must be positive
     */
    public void loadPerspective(float fovy, float aspect, float near, float far) {
        float top = near * (float) Math.tan((float) (fovy * Math.PI / 360.0f));
        float bottom = -top;
        float left = bottom * aspect;
        float right = top * aspect;
        loadFrustum(left, right, bottom, top, near, far);
    }

    /**
     * 构建一个标准化的透视投影矩阵
     * 
     * @param w screen width
     * @param h screen height
     */
    public void loadProjectionNormalized(int w, int h) {
        // range -1,1 in the narrow axis at z = 0.
        Matrix44f m1 = new Matrix44f();
        Matrix44f m2 = new Matrix44f();

        if (w > h) {
            float aspect = ((float) w) / h;
            m1.loadFrustum(-aspect, aspect, -1, 1, 1, 100);
        } else {
            float aspect = ((float) h) / w;
            m1.loadFrustum(-1, 1, -aspect, aspect, 1, 100);
        }

        m2.loadRotate(180, 0, 1, 0);
        m1.loadMultiply(m1, m2);

        m2.loadScale(-2, 2, 1);
        m1.loadMultiply(m1, m2);

        m2.loadTranslate(0, 0, 2);
        m1.loadMultiply(m1, m2);

        load(m1);
    }

    /**
     *当前矩阵左乘一个给定矩阵
     * @param rhs right hand side to multiply by
     */
    public void multiply(Matrix44f rhs) {
        Matrix44f tmp = new Matrix44f();
        tmp.loadMultiply(this, rhs);
        load(tmp);
    }

    /**
     * 对当前矩阵连接一个旋转矩阵
     * @param rot angle of rotation
     * @param x rotation axis x
     * @param y rotation axis y
     * @param z rotation axis z
     */
    public void rotate(float rot, float x, float y, float z) {
        Matrix44f tmp = new Matrix44f();
        tmp.loadRotate(rot, x, y, z);
        multiply(tmp);
    }

    /**
     * 对当前矩阵连接一个缩放矩阵(沿着坐标系的三个基准轴缩放)
     * 
     * @param x scale component x
     * @param y scale component y
     * @param z scale component z
     */
    public void scale(float x, float y, float z) {
        Matrix44f tmp = new Matrix44f();
        tmp.loadScale(x, y, z);
        multiply(tmp);
    }
    /**
     * 对当前矩阵连接一个缩放矩阵(沿着(x,y,z)方向缩放)
     * @param factor
     * @param x
     * @param y
     * @param z
     */
    public void scale(float factor,float x, float y, float z) {
        Matrix44f tmp=new Matrix44f();
        tmp.loadScale(factor, x, y, z);
        multiply(tmp);
    }

    /**
     * 对当前矩阵连接一个平移矩阵
     * @param x translation component x
     * @param y translation component y
     * @param z translation component z
     */
    public void translate(float x, float y, float z) {
        Matrix44f tmp = new Matrix44f();
        tmp.loadTranslate(x, y, z);
        multiply(tmp);
    }

    private float computeCofactor(int i, int j) {
        int c0 = (i + 1) % 4;
        int c1 = (i + 2) % 4;
        int c2 = (i + 3) % 4;
        int r0 = (j + 1) % 4;
        int r1 = (j + 2) % 4;
        int r2 = (j + 3) % 4;

        float minor = (mMat[c0 + 4 * r0] * (mMat[c1 + 4 * r1] * mMat[c2 + 4 * r2] - mMat[c1 + 4
                * r2]
                * mMat[c2 + 4 * r1]))
                - (mMat[c0 + 4 * r1] * (mMat[c1 + 4 * r0] * mMat[c2 + 4 * r2] - mMat[c1 + 4 * r2]
                        * mMat[c2 + 4 * r0]))
                + (mMat[c0 + 4 * r2] * (mMat[c1 + 4 * r0] * mMat[c2 + 4 * r1] - mMat[c1 + 4 * r1]
                        * mMat[c2 + 4 * r0]));

        float cofactor = ((i + j) & 1) != 0 ? -minor : minor;
        return cofactor;
    }

    /**
     * 矩阵求逆
     */
    public boolean inverse() {

        Matrix44f result = new Matrix44f();

        for (int i = 0; i < 4; ++i) {
            for (int j = 0; j < 4; ++j) {
                result.mMat[4 * i + j] = computeCofactor(i, j);
            }
        }

        // Dot product of 0th column of source and 0th row of result
        float det = mMat[0] * result.mMat[0] + mMat[4] * result.mMat[1] + mMat[8] * result.mMat[2]
                + mMat[12] * result.mMat[3];

        if (Math.abs(det) < 1e-6) {
            return false;
        }

        det = 1.0f / det;
        for (int i = 0; i < 16; ++i) {
            mMat[i] = result.mMat[i] * det;
        }

        return true;
    }

    /**
     * 逆转置矩阵
     */
    public boolean inverseTranspose() {

        Matrix44f result = new Matrix44f();

        for (int i = 0; i < 4; ++i) {
            for (int j = 0; j < 4; ++j) {
                result.mMat[4 * j + i] = computeCofactor(i, j);
            }
        }

        float det = mMat[0] * result.mMat[0] + mMat[4] * result.mMat[4] + mMat[8] * result.mMat[8]
                + mMat[12] * result.mMat[12];

        if (Math.abs(det) < 1e-6) {
            return false;
        }

        det = 1.0f / det;
        for (int i = 0; i < 16; ++i) {
            mMat[i] = result.mMat[i] * det;
        }

        return true;
    }

    /**
     * 转置矩阵
     */
    public void transpose() {
        for (int i = 0; i < 3; ++i) {
            for (int j = i + 1; j < 4; ++j) {
                float temp = mMat[i * 4 + j];
                mMat[i * 4 + j] = mMat[j * 4 + i];
                mMat[j * 4 + i] = temp;
            }
        }
    }
    /**
     * 将向量填充到矩阵的某一列
     * @param src
     * @param column
     */
    public void setMatrixColumn44(Vector3f src,int column) {
        set(column, 0, src.x);
        set(column, 1, src.y);
        set(column, 2, src.z);
    }
    /**
     * 将向量填充到矩阵的某一列
     * @param src
     * @param column
     */
    public void setMatrixColumn44(Vector4f src,int column) {
        set(column, 0, src.x);
        set(column, 1, src.y);
        set(column, 2, src.z);
        set(column, 3, src.w);
    }
    /**
     * 将当前矩阵的前三行三列提取出来，这个矩阵可以表示旋转和缩放
     * @return
     */
    public Matrix33f extractRotationMatrix33() {
        float[] array=new float[9];
        for(int i=0;i<3;i++){
            array[0+i*3]=mMat[0+i*4];
            array[1+i*3]=mMat[1+i*4];
            array[2+i*3]=mMat[2+i*4];
        }
        return new Matrix33f(array);
    }
    
    /**
     * 将一个3*3的矩阵替换该矩阵的前3行3列
     * @param matrix
     */
    public void replaceWithRotationMatrix(Matrix33f matrix) {
        float[] array=matrix.mMat;
        for(int i=0;i<3;i++){
            mMat[0+i*4]=array[0+i*3];
            mMat[1+i*4]=array[1+i*3];
            mMat[2+i*4]=array[2+i*3];
        }
    }
    
    final float[] mMat;
}
